/*
 * @Author: liu Shihao
 * @Date: 2022-06-12 20:54:47
 * @LastEditors: liu Shihao
 * @LastEditTime: 2022-06-16 21:50:46
 * @FilePath: \bearpi-hm_nano\applications\BearPi\BearPi-HM_Nano\sample\robot\src\scs15_control.c
 * @Description: 空闲中断
 * Copyright (c) 2022 by ${fzu} email: logic_fzu@outlook.com, All Rights Reserved.
 */
#include "robot_body.h"
#include "robot_main.h"
#include "communication_ros.h"
#include "math.h"
struct Body_t body;
struct Leg_t FR_Leg;
struct Leg_t FL_Leg;
struct Leg_t BL_Leg;
struct Leg_t BR_Leg;
//侧向力矩补偿P环
//static 	float rol_P=0;

 //初始舵机位置
int init_position[12] = { 474, 485,550,     
				          480, 540, 511, 
					      460, 551, 467,
						  500, 539, 510, }; 




void BodyInit(void)
{
    BR_Leg.IDs[0] = 7;
	BR_Leg.IDs[1] = 8;
	BR_Leg.IDs[2] = 9;
	BR_Leg.bias_position[0] = init_position[6];
	BR_Leg.bias_position[1] = init_position[7];
	BR_Leg.bias_position[2] = init_position[8];
	BR_Leg.coefficient_1 = -1;             //控制舵机顺逆旋转
	BR_Leg.coefficient_2 =  1;
	BR_Leg.coefficient_3 =  -1;
	BR_Leg.x = 0.0f;
	BR_Leg.y = 0.0f;
	BR_Leg.z = BASE_HIGH;                     //初始坐标（0，0，160）
	
	BL_Leg.IDs[0] = 10;
	BL_Leg.IDs[1] = 11;
	BL_Leg.IDs[2] = 12;
	BL_Leg.bias_position[0] = init_position[9];
	BL_Leg.bias_position[1] = init_position[10];
	BL_Leg.bias_position[2] = init_position[11];
	BL_Leg.coefficient_1 = 1;
	BL_Leg.coefficient_2 = -1;
	BL_Leg.coefficient_3 = 1;
	BL_Leg.x = 0.0f;
	BL_Leg.y = 0.0f;
	BL_Leg.z = BASE_HIGH;
	
	FL_Leg.IDs[0] = 1;
	FL_Leg.IDs[1] = 2;
	FL_Leg.IDs[2] = 3;
	FL_Leg.bias_position[0] = init_position[0];
	FL_Leg.bias_position[1] = init_position[1];
	FL_Leg.bias_position[2] = init_position[2];
	FL_Leg.coefficient_1 = -1;
	FL_Leg.coefficient_2 = -1;
	FL_Leg.coefficient_3 = 1;
	FL_Leg.x = 0.0f;
	FL_Leg.y = 0.0f;
	FL_Leg.z = BASE_HIGH;
	
	FR_Leg.IDs[0] = 4;
	FR_Leg.IDs[1] = 5;
	FR_Leg.IDs[2] = 6;
	FR_Leg.bias_position[0] = init_position[3];
	FR_Leg.bias_position[1] = init_position[4];
	FR_Leg.bias_position[2] = init_position[5];
	FR_Leg.coefficient_1 = 1;
	FR_Leg.coefficient_2 = 1;
	FR_Leg.coefficient_3 = -1;
	FR_Leg.x = 0.0f;
	FR_Leg.y = 0.0f;
	FR_Leg.z = BASE_HIGH;
	body.workstate = Stop;
}

//转换为舵机位置信号函数   
void LegCalc(struct Leg_t* leg)     
{
	     leg->vmc_flag=1;
	//根据足端轨迹位置求解关节对应位置
 	  leg->x =  leg->delta_x;      
	  leg->y = HIP + leg->delta_y;
	  leg->z = BASE_HIGH - leg->delta_z;
	//后腿加高5mm
		if(leg == &BL_Leg || leg == &BR_Leg)
		{
		 leg->z= leg->z+7;
		}
		
	float D =sqrt (leg->y * leg->y +leg->z * leg->z);
	float Lyz = sqrt (D * D  -  HIP * HIP );
	float xz=sqrt(Lyz * Lyz + leg->x * leg->x );	
	// leg->angle_1 = atan (leg->y /leg->z) - atan (HIP / D);
	// leg->angle_2 = acos((xz * xz +THIGH * THIGH - SHANK * SHANK)/(2 * THIGH * xz))- asin (leg->x/xz);
	// leg->angle_3 = acos((THIGH * THIGH + SHANK * SHANK - xz * xz)/(2 * THIGH * SHANK));
	leg->angle_1 *= Rad_to_Degree;
	leg->angle_2 *= Rad_to_Degree;
	leg->angle_3 = leg->angle_3-pi/2;
	leg->angle_3 *= Rad_to_Degree;	

	leg->position[0] = leg->bias_position[0] + leg->coefficient_1 * leg->angle_1 * Angle_to_Encoder;
	leg->position[1] = leg->bias_position[1] + leg->coefficient_2 * leg->angle_2 * Angle_to_Encoder;
	leg->position[2] = leg->bias_position[2] + leg->coefficient_3 * leg->angle_3 * Angle_to_Encoder;
}

void LegChange(void)
{
	LegCalc(&FR_Leg);
	LegCalc(&FL_Leg);
	LegCalc(&BL_Leg);
	LegCalc(&BR_Leg);
}
void ClearLegData(struct Leg_t * leg)
{
	leg->delta_x = 0;
	leg->delta_y = 0;
	leg->delta_z = 0;
}


void BodyChange(void)
{
	if(body.workstate == 1)
	{
		body.yaw = 0;
		body.pitch = 0;
		body.roll = 0;
		body.vx = 0;
		body.vy = 0;
		body.rotate = 0;

		ClearLegData(&FR_Leg);
		ClearLegData(&FL_Leg);
		ClearLegData(&BL_Leg);
		ClearLegData(&BR_Leg);
	}
	/*线性规划小跑步态 全向移动*/	
	else if(body.workstate == 4)
	{
		body.vx = remote_VX-30 ;   //前进后退
		body.vy = remote_VY-20;   //左转右转
		body.rotate =remote_VZ-20;
	    Linear_Trot();
		Linear_Trot_Gyrate(&FR_Leg);
		Linear_Trot_Gyrate(&FL_Leg);
		Linear_Trot_Gyrate(&BL_Leg);
		Linear_Trot_Gyrate(&BR_Leg);		
	}

   /*线性规划小跑步态 */
     /*添加侧向翻转*/	
	else if(body.workstate == 5)
	{

		body.vx = remote_VX-30 ;   //前进后退
		body.vy = remote_VY-20;   //左转右转
		body.rotate =remote_VZ;
	    Linear_Trot_Wide();
		Linear_Trot_Gyrate(&FR_Leg);
		Linear_Trot_Gyrate(&FL_Leg);
		Linear_Trot_Gyrate(&BL_Leg);
		Linear_Trot_Gyrate(&BR_Leg);

	}		
}




